﻿#ifndef _OI_LT360BASE_ALGORITHMS_HPP_
#define _OI_LT360BASE_ALGORITHMS_HPP_

#include <QtGui/QtGui>
#include <QMap>
#include <opencv2/opencv.hpp>
#include "OiDataStructure.hpp"
#include "concaveman.h"

Q_DECLARE_METATYPE(QList<LTBase::AlgoOutput>)
Q_DECLARE_METATYPE(LTBase::AlgoOutput)
Q_DECLARE_METATYPE(LTBase::SnapShot)
Q_DECLARE_METATYPE(std::vector<cv::Point2f>)
Q_DECLARE_METATYPE(cv::Mat)
Q_DECLARE_METATYPE(QList<QList<cv::Point2f>>)
Q_DECLARE_METATYPE(LTBase::MeasurementSnapShot)
Q_DECLARE_METATYPE(LTBase::MeasurementResults)
Q_DECLARE_METATYPE(LTBase::CalibData)


namespace LTBase
{
    #define Scale 1.0f

    //coordiante and format transformation
    inline QPointF toQPointF(cv::Point2f& point)
    {
        QPointF output = QPointF(point.x, point.y);
        return output;
    }

    inline cv::Point2f toCVPoint2f(QPointF& point)
    {
        cv::Point2f output = cv::Point2f(point.rx(), point.ry());
        return output;
    }

    //self-explanatory
    inline bool RectContainPoint(cv::Point2f& V1, cv::Point2f& V2, cv::Point2f& V3, cv::Point2f& V4, cv::Point2f& Pt)
    {
        if ((((Pt.x - V1.x) * (V2.x - V1.x) + (Pt.y - V1.y) * (V2.y - V1.y)) >= 0) &&
            (((Pt.x - V1.x) * (V4.x - V1.x) + (Pt.y - V1.y) * (V4.y - V1.y)) >= 0) &&
            (((Pt.x - V3.x) * (V2.x - V3.x) + (Pt.y - V3.y) * (V2.y - V3.y)) >= 0) &&
            (((Pt.x - V3.x) * (V4.x - V3.x) + (Pt.y - V3.y) * (V4.y - V3.y)) >= 0))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    //overloaded
    inline  bool RectContainPoint(cv::Point2f& V1, cv::Point2f& V2, cv::Point2f& V3, cv::Point2f& V4, QPointF& Pt)
    {
        if ((((Pt.rx() - V1.x) * (V2.x - V1.x) + (Pt.ry() - V1.y) * (V2.y - V1.y)) >= 0) &&
            (((Pt.rx() - V1.x) * (V4.x - V1.x) + (Pt.ry() - V1.y) * (V4.y - V1.y)) >= 0) &&
            (((Pt.rx() - V3.x) * (V2.x - V3.x) + (Pt.ry() - V3.y) * (V2.y - V3.y)) >= 0) &&
            (((Pt.rx() - V3.x) * (V4.x - V3.x) + (Pt.ry() - V3.y) * (V4.y - V3.y)) >= 0))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    inline bool RectContainPoint(QPointF& V1, QPointF& V2, QPointF& V3, QPointF& V4, QPointF& Pt)
    {
        if ((((Pt.rx() - V1.rx()) * (V2.rx() - V1.rx()) + (Pt.ry() - V1.ry()) * (V2.ry() - V1.ry())) >= 0) &&
            (((Pt.rx() - V1.rx()) * (V4.rx() - V1.rx()) + (Pt.ry() - V1.ry()) * (V4.ry() - V1.ry())) >= 0) &&
            (((Pt.rx() - V3.rx()) * (V2.rx() - V3.rx()) + (Pt.ry() - V3.ry()) * (V2.ry() - V3.ry())) >= 0) &&
            (((Pt.rx() - V3.rx()) * (V4.rx() - V3.rx()) + (Pt.ry() - V3.ry()) * (V4.ry() - V3.ry())) >= 0))
        {
            return true;
        }
        else
        {
            return false;
        }
    }

    inline int Sgn(double value)
    {
        if (Sgn > 0) return 1;
        else return -1;
    }

    inline cv::Point2f affineTransPoint(cv::Mat* TransMat, cv::Point2f& point)
    {
        return  cv::Point2f(point.x * TransMat->at<double>(0, 0) + point.y * TransMat->at<double>(0, 1) + TransMat->at<double>(0, 2),
            point.x * TransMat->at<double>(1, 0) + point.y * TransMat->at<double>(1, 1) + TransMat->at<double>(1, 2));
    }

    inline QPointF affineTransPoint(cv::Mat* TransMat, QPointF& point)
    {
        return  QPointF(point.rx() * TransMat->at<double>(0, 0) + point.ry() * TransMat->at<double>(0, 1) + TransMat->at<double>(0, 2),
            point.rx() * TransMat->at<double>(1, 0) + point.ry() * TransMat->at<double>(1, 1) + TransMat->at<double>(1, 2));
    }

    inline cv::Mat rotationMatrix(double angle)
    {
        cv::Mat Rotate = (cv::Mat_<double>(3, 3) << std::cos(angle), -std::sin(angle), 0, std::sin(angle), std::cos(angle), 0, 0, 0, 1);
        return Rotate;
    }

    inline cv::Mat translationMatrix(double tx, double ty)
    {
        cv::Mat Translate = (cv::Mat_<double>(3, 3) << 1, 0, tx, 0, 1, ty, 0, 0, 1);
        return Translate;
    }

    inline cv::Point2f projectPointToLine(const cv::Point2f& point, const cv::Point2f& point1, const cv::Point2f& point2)
    {
        double norm = cv::norm(point2 - point1);
        return point1 + (point2 - point1) * ((point.x - point1.x) * (point2.x - point1.x) + (point.y - point1.y) * (point2.y - point1.y)) / norm / norm;
    }

    inline double squaredDistance(cv::Point2f& pt1, cv::Point2f& pt2)
    {
        double x = pt1.x - pt2.x;
        double y = pt1.y - pt2.y;
        return x * x + y * y;
    }

    inline double pointToLineDistanceSquared(cv::Point2f* point, cv::Point2f* point1, cv::Point2f* point2)
    {
        return std::pow((point2->y - point1->y) * point->x - (point2->x - point1->x) * point->y + point2->x * point1->y - point2->y * point1->x, 2) / ((point2->y - point1->y) * (point2->y - point1->y) + (point2->x - point1->x) * (point2->x - point1->x));
    }

    inline void affineTransPointCloud(std::vector<cv::Point2f>* pointCloud, cv::Mat* transMat)
    {
        for (int i = 0; i < pointCloud->size(); i++)
            (*pointCloud)[i] = affineTransPoint(transMat, (*pointCloud)[i]);
    }

    inline void affineTransPointCloud(QList<QPointF>* pointCloud, cv::Mat* transMat)
    {
        for (int i = 0; i < pointCloud->size(); i++)
            (*pointCloud)[i] = toQPointF(affineTransPoint(transMat, toCVPoint2f((*pointCloud)[i])));
    }

    inline QPointF calibrationTransform(float x, float y, CalibData &calibData)
    {
        double uRaw = (x - calibData.cx) * calibData.sx;
        double vRaw = (y - calibData.cy) * calibData.sy;

        double u = 0.0;
        double v = 0.0;

        double r2 = uRaw * uRaw + vRaw * vRaw;

        if (calibData.cameraModel == CalibData::CameraModel::HALCON_DIVISION)
        {
            u = uRaw / (1.0 + calibData.k * r2) / calibData.f;
            v = vRaw / (1.0 + calibData.k * r2) / calibData.f;
        }
        else if(calibData.cameraModel == CalibData::CameraModel::HALCON_POLYNOMIAL)
        {
            double c = calibData.k1 * r2 + calibData.k2 * r2 * r2 + calibData.k3 * r2 * r2 * r2;
            u = (uRaw + uRaw * c + calibData.p2 * (r2 + 2.0 * uRaw * uRaw) + 2.0 * calibData.p1 * uRaw * vRaw) / calibData.f;
            v = (vRaw + vRaw * c + calibData.p1 * (r2 + 2.0 * vRaw * vRaw) + 2.0 * calibData.p2 * uRaw * vRaw) / calibData.f;
        }

        double denum = calibData.homography[6] * u + calibData.homography[7] * v + calibData.homography[8];
        
        return QPointF((calibData.homography[0] * u + calibData.homography[1] * v + calibData.homography[2]) / denum, (calibData.homography[3] * u + calibData.homography[4] * v + calibData.homography[5]) / denum);
    }

    inline float normSq(QPointF p)
    {
        return (p.x() * p.x() + p.y() * p.y());
    }

    //calipers
    static AlgoOutput CaliperOutSideAlgo(MeasurementTool tool, std::vector<cv::Point2f>* Centers)
    {
        QList<QPointF> inputLst = tool.data;

        AlgoOutput output;
        output.tool = tool;

        if (inputLst.size() < 4)
        {
            return output;
        }

        cv::Point2f p1(inputLst[0].rx(), inputLst[0].ry());
        cv::Point2f p2(inputLst[1].rx(), inputLst[1].ry());
        cv::Point2f p3(inputLst[2].rx(), inputLst[2].ry());
        cv::Point2f p4(inputLst[3].rx(), inputLst[3].ry());

        double x1 = p1.x;
        double x2 = p2.x;
        double x3 = p3.x;
        double x4 = p4.x;
        double y1 = p1.y;
        double y2 = p2.y;
        double y3 = p3.y;
        double y4 = p4.y;

        double r = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));//caliper length
        double width = cv::norm(p2 - p3);

        double minx = std::numeric_limits<double>::max();;
        double maxx = std::numeric_limits<double>::lowest();;
        double tempComp;//temporary distance for comparison
        bool flag = false;//detection flag

        cv::Point2f minP;
        cv::Point2f maxP;
        for (int i = 0; i < Centers->size(); i++)
        {
            if (
                ((((*Centers)[i].x - x1) * (x2 - x1) + ((*Centers)[i].y - y1) * (y2 - y1)) >= 0) &&
                ((((*Centers)[i].x - x1) * (x4 - x1) + ((*Centers)[i].y - y1) * (y4 - y1)) >= 0) &&
                ((((*Centers)[i].x - x3) * (x2 - x3) + ((*Centers)[i].y - y3) * (y2 - y3)) >= 0) &&
                ((((*Centers)[i].x - x3) * (x4 - x3) + ((*Centers)[i].y - y3) * (y4 - y3)) >= 0))
            {
                flag = true;
                tempComp = std::fabs((y4 - y1) * (*Centers)[i].x - (x4 - x1) * (*Centers)[i].y + x4 * y1 - y4 * x1) / std::sqrt((y4 - y1) * (y4 - y1) + (x4 - x1) * (x4 - x1));//distance between point and caliper jaws
                if (tempComp > maxx)
                {
                    maxx = tempComp;
                    maxP = (*Centers)[i];
                }
                if (tempComp < minx)
                {
                    minx = tempComp;
                    minP = (*Centers)[i];
                }
            }
        }

        /*double searchWindows = 5;
        int minCount, maxCount;
        minCount = 0;
        maxCount = 0;
        double minAvgx, minAvgy, maxAvgx, maxAvgy;
        minAvgx = 0;
        minAvgy = 0;
        maxAvgx = 0;
        maxAvgy = 0;
        for (int i = 0; i < Centers->size(); i++)
        {
            if (
                (std::fabs((*Centers)[i].x - minP.x) <= searchWindows) &&
                (std::fabs((*Centers)[i].y - minP.y) <= searchWindows)
                )
            {
                minAvgx = minAvgx + (*Centers)[i].x;
                minAvgy = minAvgy + (*Centers)[i].y;
                minCount++;
            }
            if (
                (std::fabs((*Centers)[i].x - maxP.x) <= searchWindows) &&
                (std::fabs((*Centers)[i].y - maxP.y) <= searchWindows)
                )
            {
                maxAvgx = maxAvgx + (*Centers)[i].x;
                maxAvgy = maxAvgy + (*Centers)[i].y;
                maxCount++;
            }
        }

        minP = cv::Point2f(minAvgx / minCount, minAvgy / minCount);
        maxP = cv::Point2f(maxAvgx / maxCount, maxAvgy / maxCount);*/

        minx = std::fabs((y4 - y1) * minP.x - (x4 - x1) * minP.y + x4 * y1 - y4 * x1) / std::sqrt((y4 - y1) * (y4 - y1) + (x4 - x1) * (x4 - x1));//distance between point and caliper jaws
        maxx = std::fabs((y4 - y1) * maxP.x - (x4 - x1) * maxP.y + x4 * y1 - y4 * x1) / std::sqrt((y4 - y1) * (y4 - y1) + (x4 - x1) * (x4 - x1));//distance between point and caliper jaws

        if (flag)
        {
            output.result = (maxx - minx) / Scale;
            output.result = output.result + tool.offset;
            output.displayData << toQPointF(cv::Point2f(x1 + (x2 - x1) * minx / r, y1 + (y2 - y1) * minx / r)) << toQPointF(cv::Point2f(x1 + (x2 - x1) * maxx / r, y1 + (y2 - y1) * maxx / r)) << toQPointF(maxP) << toQPointF(minP);
        }
        else
        {
            output.result = -1;
        }
        return output;
    }

    //caliper pairs
    static AlgoOutput CaliperPairAlgo(MeasurementTool tool, std::vector<cv::Point2f>* Centers)
    {
        QList<QPointF> inputLst = tool.data;

        AlgoOutput outputData;

        outputData.tool = tool;

        if (inputLst.size() < 8)
        {
            return outputData;
        }

        cv::Point2f a1(inputLst[0].rx(), inputLst[0].ry());
        cv::Point2f a2(inputLst[1].rx(), inputLst[1].ry());
        cv::Point2f a3(inputLst[2].rx(), inputLst[2].ry());
        cv::Point2f a4(inputLst[3].rx(), inputLst[3].ry());
        cv::Point2f b1(inputLst[4].rx(), inputLst[4].ry());
        cv::Point2f b2(inputLst[5].rx(), inputLst[5].ry());
        cv::Point2f b3(inputLst[6].rx(), inputLst[6].ry());
        cv::Point2f b4(inputLst[7].rx(), inputLst[7].ry());

        double minxA = std::numeric_limits<double>::max();
        double minxB = std::numeric_limits<double>::max();
        double tempComp;//temporary distance for comparison
        bool flagA = false;//detection flag
        bool flagB = false;

        cv::Point2f minPA, minPB;

        for (int i = 0; i < Centers->size(); i++)
        {
            if (RectContainPoint(a1, a2, a3, a4, (*Centers)[i]))
            {
                flagA = true;
                tempComp = std::fabs((a2.y - a3.y) * (*Centers)[i].x - (a2.x - a3.x) * (*Centers)[i].y + a2.x * a3.y - a2.y * a3.x) / std::sqrt((a2.y - a3.y) * (a2.y - a3.y) + (a2.x - a3.x) * (a2.x - a3.x));//distance between point and caliper jaws
                if (tempComp < minxA)
                {
                    minxA = tempComp;
                    minPA = (*Centers)[i];
                }
            }

            if (RectContainPoint(b1, b2, b3, b4, (*Centers)[i]))
            {
                flagB = true;
                tempComp = std::fabs((b2.y - b3.y) * (*Centers)[i].x - (b2.x - b3.x) * (*Centers)[i].y + b2.x * b3.y - b2.y * b3.x) / std::sqrt((b2.y - b3.y) * (b2.y - b3.y) + (b2.x - b3.x) * (b2.x - b3.x));//distance between point and caliper jaws
                if (tempComp < minxB)
                {
                    minxB = tempComp;
                    minPB = (*Centers)[i];
                }
            }
        }

        if (flagA && flagB)
        {
            /*double searchWindow = 5.0;
            int minACount, minBCount;
            minACount = 0;
            minBCount = 0;
            double minAvgx, minAvgy, minBvgx, minBvgy;
            minAvgx = 0;
            minAvgy = 0;
            minBvgx = 0;
            minBvgy = 0;
            for (int i = 0; i < Centers->size(); i++)
            {
                if (
                    (std::fabs((*Centers)[i].x - minPA.x) <= searchWindow) &&
                    (std::fabs((*Centers)[i].y - minPA.y) <= searchWindow)
                    )
                {
                    minAvgx = minAvgx + (*Centers)[i].x;
                    minAvgy = minAvgy + (*Centers)[i].y;
                    minACount++;
                }
                if (
                    (std::fabs((*Centers)[i].x - minPB.x) <= searchWindow) &&
                    (std::fabs((*Centers)[i].y - minPB.y) <= searchWindow)
                    )
                {
                    minBvgx = minBvgx + (*Centers)[i].x;
                    minBvgy = minBvgy + (*Centers)[i].y;
                    minBCount++;
                }
            }

            minPA = cv::Point2f(minAvgx / minACount, minAvgy / minACount);
            minPB = cv::Point2f(minBvgx / minBCount, minBvgy / minBCount);*/

            outputData.result = std::fabs(((minPB - minPA).x * (a1 - a2).x + (minPB - minPA).y * (a1 - a2).y)) / cv::norm(a1 - a2) / Scale;

            outputData.displayData << toQPointF(a2 + (a1 - a2) * std::sqrt(pointToLineDistanceSquared(&minPA, &a2, &a3)) / cv::norm(a1 - a2)) << toQPointF(projectPointToLine(b2 + (b1 - b2) * std::sqrt(pointToLineDistanceSquared(&minPB, &b2, &b3)) / cv::norm(b1 - b2), a2, a1)) << toQPointF(minPB) << toQPointF(minPA);
            outputData.result = outputData.result + tool.offset;

        }
        else
        {
            outputData.result = -1;
        }
        return outputData;
    }

    static AlgoOutput OuterDiameterAlgo(MeasurementTool tool, std::vector<cv::Point2f>* Centers)
    {
        //QList<QPointF> inputLst = tool.data;

        AlgoOutput output;
        output.tool = tool;

        /*if (inputLst.size() < 4)
        {
            return output;
        }

        cv::Point2f p1(inputLst[0].rx(), inputLst[0].ry());
        cv::Point2f p2(inputLst[1].rx(), inputLst[1].ry());
        cv::Point2f p3(inputLst[2].rx(), inputLst[2].ry());
        cv::Point2f p4(inputLst[3].rx(), inputLst[3].ry());

        double x1 = p1.x;
        double x2 = p2.x;
        double x3 = p3.x;
        double x4 = p4.x;
        double y1 = p1.y;
        double y2 = p2.y;
        double y3 = p3.y;
        double y4 = p4.y;

        std::vector<cv::Point2f> circle;

        for (int i = 0; i < Centers->size(); i++)
        {
            if (
                ((((*Centers)[i].x - x1) *(x2 - x1) + ((*Centers)[i].y - y1)*(y2 - y1)) >= 0) &&
                ((((*Centers)[i].x - x1) *(x4 - x1) + ((*Centers)[i].y - y1)*(y4 - y1)) >= 0) &&
                ((((*Centers)[i].x - x3) *(x2 - x3) + ((*Centers)[i].y - y3)*(y2 - y3)) >= 0) &&
                ((((*Centers)[i].x - x3) *(x4 - x3) + ((*Centers)[i].y - y3)*(y4 - y3)) >= 0))
            {
                circle.push_back((*Centers)[i]);
            }
        }*/

        if (Centers->size() < 3)
        {
            output.result = -1;
        }
        else
        {
            auto ellipse = cv::fitEllipse(*Centers);
            cv::Point2f vertices[4];
            ellipse.points(vertices);
            output.result = (ellipse.size.width + ellipse.size.height) / 2.0f;
            output.displayData << toQPointF(vertices[0]) << toQPointF(vertices[1]) << toQPointF(vertices[2]) << toQPointF(vertices[3]);
        }

        return output;
    }

    static AlgoOutput AngleMeasureAlgo(MeasurementTool tool, std::vector<cv::Point2f>* Centers)
    {
        //find two sets of line points
        //fitlines
        //compute angle
        QList<QPointF> inputLst = tool.data;

        AlgoOutput outputData;
        outputData.tool = tool;

        if (inputLst.size() < 8)
        {
            return outputData;
        }

        cv::Point2f a1(inputLst[0].rx(), inputLst[0].ry());
        cv::Point2f a2(inputLst[1].rx(), inputLst[1].ry());
        cv::Point2f a3(inputLst[2].rx(), inputLst[2].ry());
        cv::Point2f a4(inputLst[3].rx(), inputLst[3].ry());
        cv::Point2f b1(inputLst[4].rx(), inputLst[4].ry());
        cv::Point2f b2(inputLst[5].rx(), inputLst[5].ry());
        cv::Point2f b3(inputLst[6].rx(), inputLst[6].ry());
        cv::Point2f b4(inputLst[7].rx(), inputLst[7].ry());

        std::vector<cv::Point2f> linePoints1;
        std::vector<cv::Point2f> linePoints2;

        for (int i = 0; i < Centers->size(); i++)
        {
            if (RectContainPoint(a1, a2, a3, a4, (*Centers)[i]))
            {
                linePoints1.push_back((*Centers)[i]);
            }
            if (RectContainPoint(b1, b2, b3, b4, (*Centers)[i]))
            {
                linePoints2.push_back((*Centers)[i]);
            }
        }

        if (linePoints1.size() < 2 || linePoints2.size() < 2) return outputData;

        try
        {
            cv::Vec4f line1;
            cv::Vec4f line2;
            cv::fitLine(linePoints1, line1, cv::DIST_HUBER, 0, 0.01, 0.01);
            cv::fitLine(linePoints2, line2, cv::DIST_HUBER, 0, 0.01, 0.01);

            cv::Point2f vec1(line1[0], line1[1]);
            cv::Point2f vec2(line2[0], line2[1]);
            auto angle = std::acosf((vec1.x * vec2.x + vec1.y * vec2.y) / cv::norm(vec1) / cv::norm(vec2)) * 180.0f / CV_PI;
            outputData.result = angle > 90 ? 180 - angle : angle;
        }
        catch(...)
        {
        }
        
        return outputData;
    }

    static AlgoOutput FullAreaAlgo(MeasurementTool tool, std::vector<cv::Point2f>* Centers)
    {
        LTBase::AlgoOutput tempOutput;
        tempOutput.tool = tool;
        QList<QPointF> inputLst = tool.data;

        if (Centers->size() > 3)
        {
            try
            {
                std::vector<cv::Point2f> interestPoints;
                interestPoints.reserve(Centers->size());
                cv::Point2f a1(inputLst[0].rx(), inputLst[0].ry());
                cv::Point2f a2(inputLst[1].rx(), inputLst[1].ry());
                cv::Point2f a3(inputLst[2].rx(), inputLst[2].ry());
                cv::Point2f a4(inputLst[3].rx(), inputLst[3].ry());

                for (auto& point : *Centers)
                {
                    if (RectContainPoint(a1, a2, a3, a4, point))
                    {
                        bool exist = false;
                        double thres = 0.1 * 0.1;
                        for (auto& existingPoint : interestPoints)
                        {
                            double distance = (existingPoint.x - point.x) * (existingPoint.x - point.x) + (existingPoint.y - point.y) * (existingPoint.y - point.y);
                            if (distance < thres)
                            {
                                exist = true;
                            }
                        }

                        if (!exist)
                        {
                            interestPoints.push_back(point);
                        }
                    }
                }

                std::vector<int> hullIndex;
                std::vector<std::array<float, 2>> ccmanPoints;
                ccmanPoints.reserve(interestPoints.size());
                for (int i = 0; i < interestPoints.size(); i++)
                {
                    ccmanPoints.push_back(std::array<float, 2>({ interestPoints[i].x, interestPoints[i].y }));
                }
                cv::convexHull(interestPoints, hullIndex);
                auto concaveHull = CCMAN::concaveman<float, 16>(ccmanPoints, hullIndex);
                std::vector<cv::Point2f> concaveHullCV;
                concaveHullCV.reserve(concaveHull.size());
                for (auto& point : concaveHull)
                {
                    concaveHullCV.push_back(cv::Point2f(point[0], point[1]));
                }
                tempOutput.result = cv::contourArea(concaveHullCV);
            }
            catch (...)
            {
            }
        }
        return tempOutput;
    }

    static AlgoOutput measure(MeasurementTool &tool, std::vector<cv::Point2f>* Centers)
    {
        switch (tool.type)
        {
        case E_TT_OUTSIDECALIPER:
            return CaliperOutSideAlgo(tool, Centers);
        case E_TT_CALIPERPAIR:
            return CaliperPairAlgo(tool, Centers);
        case E_TT_OUTERDIAMETER:
            return OuterDiameterAlgo(tool,Centers);
        case E_TT_ANGLE:
            return AngleMeasureAlgo(tool, Centers);
        case E_TT_AREA:
            return FullAreaAlgo(tool, Centers);
        }
        return AlgoOutput();
    }

    static AlgoOutput errorMax(MeasurementTool &tool, std::vector<cv::Point2f>* profile, std::vector<cv::Point2f>* design)
    {
        if (tool.data.size() < 4)
        {
            return AlgoOutput();
        }

        if (design->size() == 0)
        {
            return AlgoOutput();
        }

        AlgoOutput output;
        output.tool = tool;
        QList<QPointF> displayData;

        cv::Point2f a = toCVPoint2f(tool.data[0]);
        cv::Point2f b = toCVPoint2f(tool.data[1]);
        cv::Point2f c = toCVPoint2f(tool.data[2]);
        cv::Point2f d = toCVPoint2f(tool.data[3]);

        double globalMaxDistance = 0;

        for (auto point : *profile)
        {
            if (RectContainPoint(a, b, c, d, point))
            {
                double localMinDistance = std::numeric_limits<double>::max();
                double tempDistance;
                cv::Point2f closestPoint;

                for (auto testPoint : *design)
                {
                    tempDistance = squaredDistance(testPoint, point);
                    if (tempDistance < localMinDistance)
                    {
                        localMinDistance = tempDistance;
                        closestPoint = testPoint;
                    }
                }

                displayData.append(toQPointF(closestPoint));
                displayData.append(toQPointF(point));

                if (globalMaxDistance < localMinDistance)
                {
                    globalMaxDistance = localMinDistance;
                }
            }
        }

        output.displayData = displayData;
        output.result = std::sqrt(globalMaxDistance) / Scale;

        return output;
    }

    static AlgoOutput errorMean(MeasurementTool &tool, std::vector<cv::Point2f>* profile, std::vector<cv::Point2f>* design)
    {
        if (tool.data.size() < 4)
        {
            return AlgoOutput();
        }

        if (design->size() == 0)
        {
            return AlgoOutput();
        }

        AlgoOutput output;
        output.tool = tool;
        QList<QPointF> displayData;

        cv::Point2f a = toCVPoint2f(tool.data[0]);
        cv::Point2f b = toCVPoint2f(tool.data[1]);
        cv::Point2f c = toCVPoint2f(tool.data[2]);
        cv::Point2f d = toCVPoint2f(tool.data[3]);

        double mean = 0;
        int count = 0;

        for (auto point : *profile)
        {
            if (RectContainPoint(a, b, c, d, point))
            {
                double localMinDistance = std::numeric_limits<double>::max();
                double tempDistance;
                cv::Point2f closestPoint;

                for (auto testPoint : *design)
                {
                    tempDistance = squaredDistance(testPoint, point);
                    if (tempDistance < localMinDistance)
                    {
                        localMinDistance = tempDistance;
                        closestPoint = testPoint;
                    }
                }

                displayData.append(toQPointF(closestPoint));
                displayData.append(toQPointF(point));

                mean = mean + localMinDistance;
                count++;
            }
        }

        output.displayData = displayData;
        output.result = mean / count / Scale;

        return output;
    }

    static AlgoOutput errorRMS(MeasurementTool &tool, std::vector<cv::Point2f>* profile, std::vector<cv::Point2f>* design)
    {
        if (tool.data.size() < 4)
        {
            return AlgoOutput();
        }

        if (design->size() == 0)
        {
            return AlgoOutput();
        }

        AlgoOutput output;
        output.tool = tool;
        QList<QPointF> displayData;

        cv::Point2f a = toCVPoint2f(tool.data[0]);
        cv::Point2f b = toCVPoint2f(tool.data[1]);
        cv::Point2f c = toCVPoint2f(tool.data[2]);
        cv::Point2f d = toCVPoint2f(tool.data[3]);

        double rootMeanSquare = 0;
        int count = 0;

        for (auto point : *profile)
        {
            if (RectContainPoint(a, b, c, d, point))
            {
                double localMinDistance = std::numeric_limits<double>::max();
                double tempDistance;
                cv::Point2f closestPoint;

                for (auto testPoint : *design)
                {
                    tempDistance = squaredDistance(testPoint, point);
                    if (tempDistance < localMinDistance)
                    {
                        localMinDistance = tempDistance;
                        closestPoint = testPoint;
                    }
                }

                displayData.append(toQPointF(closestPoint));
                displayData.append(toQPointF(point));

                rootMeanSquare = rootMeanSquare + localMinDistance * localMinDistance;
                count++;
            }
        }

        output.displayData = displayData;
        output.result = std::sqrt(rootMeanSquare / count) / Scale;

        return output;
    }

    static AlgoOutput error(MeasurementTool &tool, std::vector<cv::Point2f>* profile, std::vector<cv::Point2f>* design)
    {
        switch (tool.type)
        {
        case E_TT_ERRORMAX:
            return errorMax(tool, profile, design);
        case E_TT_ERRORMEAN:
            return errorMean(tool, profile, design);
        case E_TT_ERRORRMS:
            return errorRMS(tool, profile, design);
        default:
            return AlgoOutput();
        }
    }



    //overloaded

    static AlgoOutput error(MeasurementTool &tool, QList<QPointF>* profile, QList<QPointF>* design)
    {
        std::vector<cv::Point2f> cvprofile;
        std::vector<cv::Point2f> cvdesign;
        for (auto &point : *profile)
        {
            cvprofile.push_back(toCVPoint2f(point));
        }
        for (auto &point : *design)
        {
            cvdesign.push_back(toCVPoint2f(point));
        }
        return error(tool, &cvprofile, &cvdesign);
    }

    static AlgoOutput measure(MeasurementTool &tool, QList<QList<QPointF>>* QCenters)
    {
        std::vector<cv::Point2f> Centers;
        for (int i = 0; i < QCenters->size(); i++)
        {
            for (int j = 0; j < (*QCenters)[i].size(); j++)
            {
                Centers.push_back(cv::Point2f((*QCenters)[i][j].rx(), (*QCenters)[i][j].ry()));
            }
        }
        return measure(tool, &Centers);
    }

    static AlgoOutput measure(MeasurementTool &tool, QList<QPointF>* QCenters)
    {
        std::vector<cv::Point2f> Centers;
        for (int i = 0; i < QCenters->size(); i++)
        {
            Centers.push_back(cv::Point2f((*QCenters)[i].rx(), (*QCenters)[i].ry()));
        }
        return measure(tool, &Centers);
    }



    

    static QList<QList<QPointF>> pointCloudParser(QList<QPointF> contour, double connectionThreshold, double linkingThreshold, double noiseThreshold)
    {
        QList<QList<QPointF>> outputQList;
        QList<QPointF> tempQList;

        for (int i = 0; i < contour.size(); i++)
        {
            if (tempQList.empty())
            {
                tempQList.append(contour[i]);
            }
            else
            {
                if ((contour[i] - (tempQList)[tempQList.size() - 1]).manhattanLength() < connectionThreshold)
                {
                    tempQList.append(contour[i]);
                }
                else
                {
                    if (tempQList.size() > noiseThreshold)
                    {
                        if (outputQList.empty())
                        {
                            outputQList.append(tempQList);
                        }
                        else
                        {
                            if ((((outputQList.last()).last()) - tempQList.first()).manhattanLength() < linkingThreshold)
                            {
                                outputQList.last() = outputQList.last() + tempQList;
                            }
                            else
                            {
                                outputQList.append(tempQList);
                            }
                        }
                    }
                    tempQList.clear();
                    tempQList.append(contour[i]);
                }
            }
        }
        if (tempQList.size() > noiseThreshold)
        {
            if (outputQList.empty())
            {
                outputQList.append(tempQList);
            }
            else
            {
                if ((((outputQList.last()).last()) - tempQList.first()).manhattanLength() < linkingThreshold)
                {
                    outputQList.last() = outputQList.last() + tempQList;
                }
                else
                {
                    outputQList.append(tempQList);
                }
            }
        }
        
        return outputQList;
    }

    static void pointCloudParser(const QList<QPointF> &contour, double connectionThreshold, double linkingThreshold, double noiseThreshold, QList<QList<QPointF>> &result)
    {
        QList<QPointF> tempQList;

        double connectionThresholdSq = connectionThreshold * connectionThreshold;
        double linkingThresholdSq = linkingThreshold * linkingThreshold;

        for (int i = 0; i < contour.size(); i++)
        {
            if (tempQList.empty())
            {
                tempQList.append(contour[i]);
            }
            else
            {
                if (normSq(contour[i] - (tempQList)[tempQList.size() - 1]) < connectionThresholdSq)
                {
                    tempQList.append(contour[i]);
                }
                else
                {
                    if (tempQList.size() > noiseThreshold)
                    {
                        if (result.empty())
                        {
                            result.append(tempQList);
                        }
                        else
                        {
                            if (normSq(((result.last()).last()) - tempQList.first()) < linkingThresholdSq)
                            {
                                result.last() = result.last() + tempQList;
                            }
                            else
                            {
                                result.append(tempQList);
                            }
                        }
                    }
                    tempQList.clear();
                    tempQList.append(contour[i]);
                }
            }
        }
        if (tempQList.size() > noiseThreshold)
        {
            if (result.empty())
            {
                result.append(tempQList);
            }
            else
            {
                if (normSq(((result.last()).last()) - tempQList.first()) < linkingThresholdSq)
                {
                    result.last() = result.last() + tempQList;
                }
                else
                {
                    result.append(tempQList);
                }
            }
        }
    }


    static QList<QList<QPointF>> pointCloudParser(QList<QPointF> contour, double connectionThreshold, double linkingThreshold, double noiseThreshold, QList<int> &deletedIndex)
    {
        QList<QList<QPointF>> outputQList;
        QList<QPointF> tempQList;
        QList<int> tempIndexList;

        for (int i = 0; i < contour.size(); i++)
        {
            if (tempQList.empty())
            {
                tempQList.append(contour[i]);
                tempIndexList.append(i);
            }
            else
            {
                if ((contour[i] - (tempQList)[tempQList.size() - 1]).manhattanLength() < connectionThreshold)
                {
                    tempQList.append(contour[i]);
                    tempIndexList.append(i);
                }
                else
                {
                    if (tempQList.size() > noiseThreshold)
                    {
                        if (outputQList.empty())
                        {
                            outputQList.append(tempQList);
                        }
                        else
                        {
                            if ((((outputQList.last()).last()) - tempQList.first()).manhattanLength() < linkingThreshold)
                            {
                                outputQList.last() = outputQList.last() + tempQList;
                            }
                            else
                            {
                                outputQList.append(tempQList);
                            }
                        }
                    }
                    else
                    {
                        deletedIndex.append(tempIndexList);
                    }
                    tempQList.clear();
                    tempQList.append(contour[i]);
                    tempIndexList.clear();
                    tempIndexList.append(i);

                }
            }
        }
        if (tempQList.size() > noiseThreshold)
        {
            if (outputQList.empty())
            {
                outputQList.append(tempQList);
            }
            else
            {
                if ((((outputQList.last()).last()) - tempQList.first()).manhattanLength() < linkingThreshold)
                {
                    outputQList.last() = outputQList.last() + tempQList;
                }
                else
                {
                    outputQList.append(tempQList);
                }
            }
        }
        else
        {
            deletedIndex.append(tempIndexList);
        }

        return outputQList;
    }

    //algorithms overloaded with QList<QList<QPointF>>, can be called dicrectly from UI
    //some cleanup needed

   
    static double lineAngle(QList<QPointF>& rectangle, QList<QPointF>& Centers, QList<QPointF>* outputLine)
    {
        assert(rectangle.size() >= 4 && "lineAngle wrong input");

        cv::Point2f a1(rectangle[0].rx(), rectangle[0].ry());
        cv::Point2f a2(rectangle[1].rx(), rectangle[1].ry());
        cv::Point2f a3(rectangle[2].rx(), rectangle[2].ry());
        cv::Point2f a4(rectangle[3].rx(), rectangle[3].ry());

        std::vector<cv::Point2f> line;

        float minx = std::numeric_limits<float>::max();
        float miny = std::numeric_limits<float>::max();
        float maxx = std::numeric_limits<float>::lowest();
        float maxy = std::numeric_limits<float>::lowest();

        for (int i = 0; i < Centers.size(); i++)
        {
            if (
                (((Centers[i].x() - a1.x) * (a2.x - a1.x) + (Centers[i].y() - a1.y) * (a2.y - a1.y)) >= 0) &&
                (((Centers[i].x() - a1.x) * (a4.x - a1.x) + (Centers[i].y() - a1.y) * (a4.y - a1.y)) >= 0) &&
                (((Centers[i].x() - a3.x) * (a2.x - a3.x) + (Centers[i].y() - a3.y) * (a2.y - a3.y)) >= 0) &&
                (((Centers[i].x() - a3.x) * (a4.x - a3.x) + (Centers[i].y() - a3.y) * (a4.y - a3.y)) >= 0))
            {
                line.push_back(cv::Point2f(Centers[i].x(), Centers[i].y()));
                if (Centers[i].x() < minx) minx = Centers[i].x();
                if (Centers[i].y() < miny) miny = Centers[i].y();
                if (Centers[i].x() > maxx) maxx = Centers[i].x();
                if (Centers[i].y() > maxy) maxy = Centers[i].y();
            }
        }

        if (line.size() > 0)
        {
            cv::Vec4f lineFit;
            cv::fitLine(line, lineFit, CV_DIST_L2, 0, 0.01, 0.01);

            if (outputLine != NULL)
            {
                if (std::fabs(lineFit[0]) > std::fabs(lineFit[1]))
                {
                    outputLine->append(QPointF(minx, lineFit[3] - (lineFit[2] - minx) * lineFit[1] / lineFit[0]));
                    outputLine->append(QPointF(maxx, lineFit[3] - (lineFit[2] - maxx) * lineFit[1] / lineFit[0]));
                }
                else
                {
                    outputLine->append(QPointF(lineFit[2] - (lineFit[3] - miny) * lineFit[0] / lineFit[1], miny));
                    outputLine->append(QPointF(lineFit[2] - (lineFit[3] - maxy) * lineFit[0] / lineFit[1], maxy));
                }
            }

            return (fabs(lineFit[0] * lineFit[1]) / (lineFit[0] * lineFit[1])) * std::acos(fabs(lineFit[0]));

        }

        return 0;
    }

    static double lineAngle(QList<QPointF>& rectangle, QList<QList<QPointF>>& Centers, QList<QPointF>* outputLine)
    {
        assert(rectangle.size() >= 4 && "lineAngle wrong input");

        cv::Point2f a1(rectangle[0].rx(), rectangle[0].ry());
        cv::Point2f a2(rectangle[1].rx(), rectangle[1].ry());
        cv::Point2f a3(rectangle[2].rx(), rectangle[2].ry());
        cv::Point2f a4(rectangle[3].rx(), rectangle[3].ry());

        std::vector<cv::Point2f> line;

        float minx = std::numeric_limits<float>::max();
        float miny = std::numeric_limits<float>::max();
        float maxx = std::numeric_limits<float>::lowest();
        float maxy = std::numeric_limits<float>::lowest();

        for (int i = 0; i < Centers.size(); i++)
        {
            for (int j = 0; j < Centers[i].size(); j++)
            {
                if (
                    (((Centers[i][j].x() - a1.x) * (a2.x - a1.x) + (Centers[i][j].y() - a1.y) * (a2.y - a1.y)) >= 0) &&
                    (((Centers[i][j].x() - a1.x) * (a4.x - a1.x) + (Centers[i][j].y() - a1.y) * (a4.y - a1.y)) >= 0) &&
                    (((Centers[i][j].x() - a3.x) * (a2.x - a3.x) + (Centers[i][j].y() - a3.y) * (a2.y - a3.y)) >= 0) &&
                    (((Centers[i][j].x() - a3.x) * (a4.x - a3.x) + (Centers[i][j].y() - a3.y) * (a4.y - a3.y)) >= 0))
                {
                    line.push_back(cv::Point2f(Centers[i][j].x(), Centers[i][j].y()));
                    if (Centers[i][j].x() < minx) minx = Centers[i][j].x();
                    if (Centers[i][j].y() < miny) miny = Centers[i][j].y();
                    if (Centers[i][j].x() > maxx) maxx = Centers[i][j].x();
                    if (Centers[i][j].y() > maxy) maxy = Centers[i][j].y();
                }
            }
        }

        if (line.size() > 0)
        {
            cv::Vec4f lineFit;
            cv::fitLine(line, lineFit, CV_DIST_L2, 0, 0.01, 0.01);

            if (outputLine != NULL)
            {
                if (std::fabs(lineFit[0]) > std::fabs(lineFit[1]))
                {
                    outputLine->append(QPointF(minx, lineFit[3] - (lineFit[2] - minx) * lineFit[1] / lineFit[0]));
                    outputLine->append(QPointF(maxx, lineFit[3] - (lineFit[2] - maxx) * lineFit[1] / lineFit[0]));
                }
                else
                {
                    outputLine->append(QPointF(lineFit[2] - (lineFit[3] - miny) * lineFit[0] / lineFit[1], miny));
                    outputLine->append(QPointF(lineFit[2] - (lineFit[3] - maxy) * lineFit[0] / lineFit[1], maxy));
                }
            }

            return (fabs(lineFit[0] * lineFit[1]) / (lineFit[0] * lineFit[1])) * std::acos(fabs(lineFit[0]));
        }

        return 0;
    }

    static double lineAngle(QList<QPointF>& rectangle, std::vector<cv::Point2f>* Centers)
    {
        assert(rectangle.size() >= 4 && "lineAngle wrong input");

        cv::Point2f a1(rectangle[0].rx(), rectangle[0].ry());
        cv::Point2f a2(rectangle[1].rx(), rectangle[1].ry());
        cv::Point2f a3(rectangle[2].rx(), rectangle[2].ry());
        cv::Point2f a4(rectangle[3].rx(), rectangle[3].ry());

        std::vector<cv::Point2f> line;

        for (int i = 0; i < Centers->size(); i++)
        {
            if (
                ((((*Centers)[i].x - a1.x) * (a2.x - a1.x) + ((*Centers)[i].y - a1.y) * (a2.y - a1.y)) >= 0) &&
                ((((*Centers)[i].x - a1.x) * (a4.x - a1.x) + ((*Centers)[i].y - a1.y) * (a4.y - a1.y)) >= 0) &&
                ((((*Centers)[i].x - a3.x) * (a2.x - a3.x) + ((*Centers)[i].y - a3.y) * (a2.y - a3.y)) >= 0) &&
                ((((*Centers)[i].x - a3.x) * (a4.x - a3.x) + ((*Centers)[i].y - a3.y) * (a4.y - a3.y)) >= 0))
            {
                line.push_back((*Centers)[i]);
            }
        }

        if (line.size() > 0)
        {
            cv::Vec4f lineFit;
            cv::fitLine(line, lineFit, CV_DIST_L2, 0, 0.01, 0.01);
            return (fabs(lineFit[0] * lineFit[1]) / (lineFit[0] * lineFit[1])) * std::acos(fabs(lineFit[0]));
        }

        return 0;

    }
    

   


    static cv::Mat alignLineLeft(QList<QPointF> &rectangle, QList<QPointF> &Centers, QList<QPointF> *line = NULL, QPointF *alignPoint = NULL)
    {
        assert(rectangle.size() >= 4 && "alignLineLeft wrong input");

        QPointF origin = (rectangle[0] + rectangle[2]) / 2.0;

        double angle = lineAngle(rectangle, Centers, line);

        cv::Mat homRotation = rotationMatrix(-angle) * translationMatrix(-origin.x(),-origin.y());

        float xmin = std::numeric_limits<float>::max();

        for (auto &point : Centers)
        {
            point = affineTransPoint(&homRotation, point);
            if (point.x() < xmin)
            {
                xmin = point.x();
                if (alignPoint != NULL)
                {
                    *alignPoint = QPointF(point);
                }
            }
        }

        for (auto &point : Centers)
        {
            point.rx() = point.x() - xmin;
        }

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(alignPoint->x() - xmin, alignPoint->y());
        }

        cv::Mat finalTransform = translationMatrix(-xmin, 0) * homRotation;

        if (line != NULL)
        {
            if (line->size() >= 2)
            {
                (*line)[0] = affineTransPoint(&finalTransform, (*line)[0]);
                (*line)[1] = affineTransPoint(&finalTransform, (*line)[1]);
            }
        }

        return finalTransform;
    }

    static cv::Mat alignLineLeft(QList<QPointF> &rectangle, QList<QList<QPointF>> &Centers, QList<QPointF> *line = NULL, QPointF *alignPoint = NULL)
    {
        assert(rectangle.size() >= 4 && "alignLineLeft wrong input");

        QPointF origin = (rectangle[0] + rectangle[2]) / 2.0;

        double angle = lineAngle(rectangle, Centers, line);

        cv::Mat homRotation = rotationMatrix(-angle) * translationMatrix(-origin.x(), -origin.y());

        float xmin = std::numeric_limits<float>::max();

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point = affineTransPoint(&homRotation, point);
                if (point.x() < xmin)
                {
                    xmin = point.x();
                    if (alignPoint != NULL)
                    {
                        *alignPoint = QPointF(point);
                    }
                }
            }
        }

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point.rx() = point.x() - xmin;
            }
        }

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(alignPoint->x() - xmin, alignPoint->y());
        }

        cv::Mat finalTransform = translationMatrix(-xmin, 0) * homRotation;

        if (line != NULL)
        {
            if (line->size() >= 2)
            {
                (*line)[0] = affineTransPoint(&finalTransform, (*line)[0]);
                (*line)[1] = affineTransPoint(&finalTransform, (*line)[1]);
            }
        }

        return finalTransform;
    }

    static cv::Mat alignLineRight(QList<QPointF> &rectangle, QList<QPointF> &Centers, QList<QPointF> *line = NULL, QPointF *alignPoint = NULL)
    {
        assert(rectangle.size() >= 4 && "alignLineLeft wrong input");

        QPointF origin = (rectangle[0] + rectangle[2]) / 2.0;

        double angle = lineAngle(rectangle, Centers, line);

        cv::Mat homRotation = rotationMatrix(-angle) * translationMatrix(-origin.x(), -origin.y());

        float xmax = -std::numeric_limits<float>::max();

        for (auto &point : Centers)
        {
            point = affineTransPoint(&homRotation, point);
            if (point.x() > xmax)
            {
                xmax = point.x();
                if (alignPoint != NULL)
                {
                    *alignPoint = QPointF(point);
                }
            }
        }

        for (auto &point : Centers)
        {
            point.rx() = point.x() - xmax;
        }

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(alignPoint->x() - xmax, alignPoint->y());
        }

        cv::Mat finalTransform = translationMatrix(-xmax, 0) * homRotation;

        if (line != NULL)
        {
            if (line->size() >= 2)
            {
                (*line)[0] = affineTransPoint(&finalTransform, (*line)[0]);
                (*line)[1] = affineTransPoint(&finalTransform, (*line)[1]);
            }
        }

        return finalTransform;
    }

    static cv::Mat alignLineRight(QList<QPointF> &rectangle, QList<QList<QPointF>> &Centers, QList<QPointF> *line = NULL, QPointF *alignPoint = NULL)
    {
        assert(rectangle.size() >= 4 && "alignLineLeft wrong input");

        QPointF origin = (rectangle[0] + rectangle[2]) / 2.0;

        double angle = lineAngle(rectangle, Centers, line);

        cv::Mat homRotation = rotationMatrix(-angle) * translationMatrix(-origin.x(), -origin.y());

        float xmax = -std::numeric_limits<float>::max();

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point = affineTransPoint(&homRotation, point);
                if (point.x() > xmax)
                {
                    xmax = point.x();
                    if (alignPoint != NULL)
                    {
                        *alignPoint = QPointF(point);
                    }
                }
            }
        }

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point.rx() = point.x() - xmax;
            }
        }

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(alignPoint->x() - xmax, alignPoint->y());
        }

        cv::Mat finalTransform = translationMatrix(-xmax, 0) * homRotation;

        if (line != NULL)
        {
            if (line->size() >= 2)
            {
                (*line)[0] = affineTransPoint(&finalTransform, (*line)[0]);
                (*line)[1] = affineTransPoint(&finalTransform, (*line)[1]);
            }
        }

        return finalTransform;
    }

   

    static QPointF extractPoint(QList<QPointF> &rectangle, QList<QPointF> &Centers)
    {
        assert(rectangle.size() >= 4 && "lineAngle wrong input");

        QPointF a1 = rectangle[0];
        QPointF a2 = rectangle[1];
        QPointF a3 = rectangle[2];
        QPointF a4 = rectangle[3];
       
        double minx = std::numeric_limits<double>::max();

        double tempComp;//temporary distance for comparison
        bool flag = false;//detection flag

        QPointF minP = QPointF(0, 0);

        for (int i = 0; i < Centers.size(); i++)
        {
            if (RectContainPoint(a1, a2, a3, a4, Centers[i]))
            {
                flag = true;
                tempComp = std::fabs((a2.y() - a3.y())*Centers[i].x() - (a2.x() - a3.x())*Centers[i].y() + a2.x()*a3.y() - a2.y()*a3.x()) / std::sqrt((a2.y() - a3.y())*(a2.y() - a3.y()) + (a2.x() - a3.x())*(a2.x() - a3.x()));//distance between point and caliper jaws
                if (tempComp < minx)
                {
                    minx = tempComp;
                    minP = Centers[i];
                }
            }
        }

        if (false)
        {
            double searchWindow = 5.0;
            int count = 0;
            double minAvgx, minAvgy;
            minAvgx = 0;
            minAvgy = 0;
            for (int i = 0; i < Centers.size(); i++)
            {
                if (
                    (std::fabs(Centers[i].x() - minP.x()) <= searchWindow) &&
                    (std::fabs(Centers[i].y() - minP.y()) <= searchWindow)
                    )
                {
                    minAvgx = minAvgx + Centers[i].x();
                    minAvgy = minAvgy + Centers[i].y();
                    count++;
                }
            }

            minP = QPointF(minAvgx / count, minAvgy / count);
        }

        return minP;
    }

    static QPointF extractPoint(QList<QPointF> &rectangle, QList<QList<QPointF>> &Centers)
    {
        assert(rectangle.size() >= 4 && "lineAngle wrong input");

        QPointF a1 = rectangle[0];
        QPointF a2 = rectangle[1];
        QPointF a3 = rectangle[2];
        QPointF a4 = rectangle[3];

        double minx = std::numeric_limits<double>::max();

        double tempComp;//temporary distance for comparison
        bool flag = false;//detection flag

        QPointF minP = QPointF(0,0);

        for (int i = 0; i < Centers.size(); i++)
        {
            for (int j = 0; j < Centers[i].size(); j++)
            {
                if (RectContainPoint(a1, a2, a3, a4, Centers[i][j]))
                {
                    flag = true;
                    tempComp = std::fabs((a2.y() - a3.y())*Centers[i][j].x() - (a2.x() - a3.x())*Centers[i][j].y() + a2.x()*a3.y() - a2.y()*a3.x()) / std::sqrt((a2.y() - a3.y())*(a2.y() - a3.y()) + (a2.x() - a3.x())*(a2.x() - a3.x()));//distance between point and caliper jaws
                    if (tempComp < minx)
                    {
                        minx = tempComp;
                        minP = Centers[i][j];
                    }
                }
            }
            
        }

        if (false)
        {
            if (flag)
            {
                double searchWindow = 5.0;
                int count = 0;
                double minAvgx, minAvgy;
                minAvgx = 0;
                minAvgy = 0;
                for (int i = 0; i < Centers.size(); i++)
                {
                    for (int j = 0; j < Centers[i].size(); j++)
                    {
                        if (
                            (std::fabs(Centers[i][j].x() - minP.x()) <= searchWindow) &&
                            (std::fabs(Centers[i][j].y() - minP.y()) <= searchWindow)
                            )
                        {
                            minAvgx = minAvgx + Centers[i][j].x();
                            minAvgy = minAvgy + Centers[i][j].y();
                            count++;
                        }
                    }
                   
                }

                minP = QPointF(minAvgx / count, minAvgy / count);
            }
        }

        return minP;
    }

    static cv::Mat alignLinePoint(QList<QPointF> &rectangles, QList<QPointF> &Centers, QList<QPointF> *line = NULL, QPointF *alignPoint = NULL)
    {
        assert(rectangles.size() >= 8 && "alignLinePoint wrong input");

        QPointF origin = (rectangles[0] + rectangles[2]) / 2.0;

        QList<QPointF> lineRectangle = rectangles.mid(0,4);

        double angle = lineAngle(lineRectangle, Centers, line);

        cv::Mat homRotation = rotationMatrix(-angle) * translationMatrix(-origin.x(), -origin.y());

        for (auto &point : Centers)
        {
            point = affineTransPoint(&homRotation, point);
        }

        QList<QPointF> pointSearchRectangle;

        for (int i = 4; i < 8; i++)
        {
            pointSearchRectangle.append(affineTransPoint(&homRotation, rectangles[i]));
        }

        QPointF extractedPoint = extractPoint(pointSearchRectangle, Centers);

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(extractedPoint);
        }

        for (auto &point : Centers)
        {
            point.rx() = point.x() - extractedPoint.x();
        }

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(alignPoint->x() - extractedPoint.x(), alignPoint->y());
        }

        cv::Mat finalTransform = translationMatrix(-extractedPoint.x(), 0) * homRotation;

        if (line != NULL)
        {
            if (line->size() >= 2)
            {
                (*line)[0] = affineTransPoint(&finalTransform, (*line)[0]);
                (*line)[1] = affineTransPoint(&finalTransform, (*line)[1]);
            }
        }

        return finalTransform;
    }

    static cv::Mat alignLinePoint(QList<QPointF> &rectangles, QList<QList<QPointF>> &Centers, QList<QPointF> *line = NULL, QPointF *alignPoint = NULL)
    {
        assert(rectangles.size() >= 8 && "alignLinePoint wrong input");

        QPointF origin = (rectangles[0] + rectangles[2]) / 2.0;

        QList<QPointF> lineRectangle = rectangles.mid(0, 4);

        double angle = lineAngle(lineRectangle, Centers, line);

        cv::Mat homRotation = rotationMatrix(-angle) * translationMatrix(-origin.x(), -origin.y());

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point = affineTransPoint(&homRotation, point);
            }
        }

        QList<QPointF> pointSearchRectangle;

        for (int i = 4; i < 8; i++)
        {
            pointSearchRectangle.append(affineTransPoint(&homRotation, rectangles[i]));
        }

        QPointF extractedPoint = extractPoint(pointSearchRectangle, Centers);

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(extractedPoint);
        }

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point.rx() = point.x() - extractedPoint.x();
            }
        }

        if (alignPoint != NULL)
        {
            *alignPoint = QPointF(alignPoint->x() - extractedPoint.x(), alignPoint->y());
        }

        cv::Mat finalTransform = translationMatrix(-extractedPoint.x(), 0) * homRotation;

        if (line != NULL)
        {
            if (line->size() >= 2)
            {
                (*line)[0] = affineTransPoint(&finalTransform, (*line)[0]);
                (*line)[1] = affineTransPoint(&finalTransform, (*line)[1]);
            }
        }

        return finalTransform;
    }

    static cv::Mat alignThreePoints(QList<QPointF> &rectangles, QList<QPointF> &Centers, QList<QPointF> *points = NULL)
    {
        assert(rectangles.size() >= 12 && "alignThreePoints wrong input");

        QList<QPointF> pivotRectangle = rectangles.mid(0, 4);
        QPointF pivot = (pivotRectangle[0] + pivotRectangle[2]) / 2.0;

        QList<QPointF> rotateRectangle = rectangles.mid(4, 4);
        QList<QPointF> translateRectangle = rectangles.mid(8, 4);

        QPointF pivotPoint = extractPoint(pivotRectangle, Centers);
        QPointF rotatePoint = extractPoint(rotateRectangle, Centers);

        QPointF translatePoint = extractPoint(translateRectangle, Centers);

        QPointF rotateVector = rotatePoint - pivotPoint;

        double angle = std::atan(rotateVector.y() / rotateVector.x());

        cv::Mat homRotate = rotationMatrix(-angle) * translationMatrix(-pivotPoint.x(), -pivotPoint.y());

        for (auto &point : Centers)
        {
            point = affineTransPoint(&homRotate, point);
        }

        translatePoint = affineTransPoint(&homRotate, translatePoint);

        cv::Mat translation = translationMatrix(-translatePoint.x(), 0);

        for (auto &point : Centers)
        {
            point = affineTransPoint(&translation, point);
        }

        cv::Mat transform = translation * homRotate;

        if (points != NULL)
        {
            points->append(affineTransPoint(&transform, pivotPoint));
            points->append(affineTransPoint(&transform, rotatePoint));
            points->append(affineTransPoint(&translation, translatePoint));
        }

        return transform;
       
    }

    static cv::Mat alignThreePoints(QList<QPointF> &rectangles, QList<QList<QPointF>> &Centers, QList<QPointF>* points = NULL)
    {
        assert(rectangles.size() >= 12 && "alignThreePoints wrong input");

        QList<QPointF> pivotRectangle = rectangles.mid(0, 4);
        QPointF pivot = (pivotRectangle[0] + pivotRectangle[2]) / 2.0;

        QList<QPointF> rotateRectangle = rectangles.mid(4, 4);
        QList<QPointF> translateRectangle = rectangles.mid(8, 4);

        QPointF translatePoint = extractPoint(translateRectangle, Centers);

        QPointF pivotPoint = extractPoint(pivotRectangle, Centers);
        QPointF rotatePoint = extractPoint(rotateRectangle, Centers);

        QPointF rotateVector = rotatePoint - pivotPoint;

        double angle = std::atan(rotateVector.y() / rotateVector.x());

        cv::Mat homRotate = rotationMatrix(-angle) * translationMatrix(-pivotPoint.x(), -pivotPoint.y());

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point = affineTransPoint(&homRotate, point);
            }
        }

        translatePoint = affineTransPoint(&homRotate, translatePoint);

        cv::Mat translation = translationMatrix(-translatePoint.x(), 0);

        for (auto &contour : Centers)
        {
            for (auto &point : contour)
            {
                point = affineTransPoint(&translation, point);
            }
        }

        cv::Mat transform = translation * homRotate;

        if (points != NULL)
        {
            points->append(affineTransPoint(&transform, pivotPoint));
            points->append(affineTransPoint(&transform, rotatePoint));
            points->append(affineTransPoint(&translation, translatePoint));
        }

        return transform;

    }
}

#endif